import { psr_to_xyz, matmul, vector4to3 } from '../utils/data'

// https://ww2.mathworks.cn/help/vision/ug/camera-calibration.html?#responsive_offcanvas

export function box_to_2d_points(box, calib) {
  if (!box) return null
  var scale = box.scale
  var pos = box.position
  var rotation = box.rotation

  var box3d = psr_to_xyz(pos, scale, rotation)
  var imgpos = matmul(calib.extrinsic, box3d, 4)

  var imgpos3 = vector4to3(imgpos)

  var imgpos2
  if (calib.intrinsic.length > 9) {
    imgpos2 = matmul(calib.intrinsic, imgpos, 4)
  } else {
    if (calib.xi) {
      imgpos2 = omnidirectional(calib.intrinsic, calib.distortion, calib.xi, imgpos3)
    } else if (calib.distortion) {
      imgpos2 = radtan(calib.intrinsic, calib.distortion, imgpos3)
    } else {
      imgpos2 = matmul(calib.intrinsic, imgpos3, 3)
    }
  }

  if (!all_points_in_image_range(imgpos3)) {
    return null
  }
  var imgfinal = vector3_nomalize(imgpos2)

  return imgfinal
}

function all_points_in_image_range(p) {
  for (var i = 0; i < p.length / 3; i++) {
    if (p[i * 3 + 2] < 0) {
      return false
    }
  }

  return true
}

function vector3_nomalize(m) {
  var ret = []
  for (var i = 0; i < m.length / 3; i++) {
    ret.push(m[i * 3 + 0] / m[i * 3 + 2])
    ret.push(m[i * 3 + 1] / m[i * 3 + 2])
  }

  return ret
}

// Omnidirectional Camera, Radial Distortion & Tangential Distortion
// https://rpg.ifi.uzh.ch/docs/omnidirectional_camera.pdf
function omnidirectional(intrinsic, distortion, xi, points) {
  const ret = []
  for (let ii = 0; ii < points.length / 3; ii++) {
    const x = points[ii * 3],
      y = points[ii * 3 + 1],
      z = points[ii * 3 + 2],
      k1 = distortion[0],
      k2 = distortion[1],
      p1 = distortion[2],
      p2 = distortion[3]
    const xs = x / Math.sqrt(x * x + y * y + z * z)
    const ys = y / Math.sqrt(x * x + y * y + z * z)
    const zs = z / Math.sqrt(x * x + y * y + z * z)
    const xm = xs / (zs + xi)
    const ym = ys / (zs + xi)
    const zm = zs + xi

    const r = Math.sqrt(xm * xm + ym * ym)
    const xRadialDistortion = xm * (1 + k1 * Math.pow(r, 2) + k2 * Math.pow(r, 4))
    const yRadialDistortion = ym * (1 + k1 * Math.pow(r, 2) + k2 * Math.pow(r, 4))
    const xTangentialDistortion = 2 * p1 * xm * ym + p2 * (r * r + 2 * xm * xm)
    const yTangentialDistortion = p1 * (r * r + 2 * ym * ym) + 2 * p2 * xm * ym
    const x2 = xRadialDistortion + xTangentialDistortion
    const y2 = yRadialDistortion + yTangentialDistortion

    points[ii * 3] = x2 * zm
    points[ii * 3 + 1] = y2 * zm
    points[ii * 3 + 2] = zm

    for (let jj = 0; jj < 3; jj++) {
      ret[ii * 3 + jj] = 0
      for (let kk = 0; kk < 3; kk++) {
        ret[ii * 3 + jj] += intrinsic[jj * 3 + kk] * points[ii * 3 + kk]
      }
    }
  }

  return ret
}

// Pinhole Camera Model, Radial Distortion & Tangential Distortion
// https://ww2.mathworks.cn/help/vision/ug/camera-calibration.html?#bu0nj3f
function radtan(intrinsic, distortion, points) {
  const ret = []
  for (let ii = 0; ii < points.length / 3; ii++) {
    const x = points[ii * 3],
      y = points[ii * 3 + 1],
      z = points[ii * 3 + 2],
      k1 = distortion[0],
      k2 = distortion[1],
      p1 = distortion[2],
      p2 = distortion[3]
    x = x / z
    y = y / z
    const r = Math.sqrt(x * x + y * y)
    const xRadialDistortion = x * (1 + k1 * Math.pow(r, 2) + k2 * Math.pow(r, 4))
    const yRadialDistortion = y * (1 + k1 * Math.pow(r, 2) + k2 * Math.pow(r, 4))
    const xTangentialDistortion = 2 * p1 * x * y + p2 * (r * r + 2 * x * x)
    const yTangentialDistortion = p1 * (r * r + 2 * y * y) + 2 * p2 * x * y
    const x2 = xRadialDistortion + xTangentialDistortion
    const y2 = yRadialDistortion + yTangentialDistortion

    points[ii * 3] = x2 * z
    points[ii * 3 + 1] = y2 * z
    points[ii * 3 + 2] = z

    for (let jj = 0; jj < 3; jj++) {
      ret[ii * 3 + jj] = 0
      for (let kk = 0; kk < 3; kk++) {
        ret[ii * 3 + jj] += intrinsic[jj * 3 + kk] * points[ii * 3 + kk]
      }
    }
  }

  return ret
}

// Equidistant (EQUI)
function equidistant(intrinsic, distortion, points) {
  const fx = intrinsic[0]
  const fy = intrinsic[4]
  const cx = intrinsic[2]
  const cy = intrinsic[5]
  const ret = []
  for (let ii = 0; ii < points.length / 3; ii++) {
    const x = points[ii * 3],
      y = points[ii * 3 + 1]
    const r = Math.sqrt(x * x + y * y)

    const theta = Math.atan(r)

    const theta_d =
      theta *
      (1 +
        distortion[0] * Math.pow(theta, 2) +
        distortion[1] * Math.pow(theta, 4) +
        distortion[2] * Math.pow(theta, 6) +
        distortion[3] * Math.pow(theta, 8))

    const scaling = r > 1e-8 ? theta_d / r : 1.0
    ret.push(fx * x * scaling + cx)
    ret.push(fy * y * scaling + cy)
    ret.push(1)
  }

  return ret
}
